{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# Epsilon Tutorial\n",
    "\n",
    "This notebook describes the epsilon mechanism by which numerical singularities are handled in SymForce. The [paper](https://arxiv.org/abs/2204.07889) addresses the theory in Section VI, and this tutorial demonstrates the idea through examples.\n",
    "\n",
    "The basic concept is that it is common to have functions in robotics that are smooth but have singularities at given points. Handwritten functions tend to handle them by adding an if statement at the singularity with some kind of approximation or alternate formulation. This is harder to do with symbolic expressions, and also is not kind to branch prediction. SymForce addresses this with a different method - shifting the input to the function away from the singular point with an infinitesimal variable $\\epsilon$ (epsilon). This approach is simple and fast for a useful class of removable singularities, with negligible effect to output values for sufficiently small epsilon.\n",
    "\n",
    "All functions in SymForce that have singularities take epsilon as an argument. In a numerical context, a very small floating point number should be passed in. In the symbolic context, an epsilon symbol should be passed in. Epsilon arguments are currently optional with zero defaults. This is convenient so that playing with expressions in a notebook doesn't require passing epsilons around. However, this is dangerous and it is extremely important to pass epsilons to get robust behavior or when generating code. Because of this, there are active efforts to make a more intelligent mechanism for the default epsilon to make it less of a footgun to accidentally forget an epsilon and end up with a NaN."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Goal\n",
    "\n",
    "We have a function `f(x)`.\n",
    "\n",
    "In the simplest case, we're trying to fix a removable singularity at `x=0`, i.e. `f(x).subs(x, 0) == sm.S.NaN`\n",
    "\n",
    "Libraries often do this by checking whether the value of `x` is close to 0, and using a different method for evaluation there, such as a Taylor expansion.  In symbolic expressions, branching like this is messy and expensive.\n",
    "\n",
    "The idea is to instead make a function `f(x, eps)` so the value is not NaN, when `eps` is a small positive number:  \n",
    "`f(x, eps).subs(x, 0) != sm.S.NaN`\n",
    "\n",
    "We usually also want that the derivative is not NaN:  \n",
    "`f(x, eps).diff(x).subs(x, 0) != NaN`\n",
    "\n",
    "For value continuity we want to match the limit:  \n",
    "`f(x, eps).subs(x, 0).limit(eps, 0) == f(x).limit(x, 0)`\n",
    "\n",
    "For derivative continuity we want to match the limit: \n",
    "`f(x, eps).diff(x).subs(x, 0).limit(eps, 0) == f(x).diff(x).limit(x, 0)`"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import numpy as np\n",
    "import plotly.express as px\n",
    "import sympy as sm\n",
    "\n",
    "x = sm.Symbol(\"x\")\n",
    "eps = sm.Symbol(\"epsilon\")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## An example: sin(x) / x\n",
    "\n",
    "For the whole section below, let's pretend x is positive so $x = -\\epsilon$ is not a fear. We'll address that later."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# The function `sin(x) / x` looks like this:\n",
    "def f(x):\n",
    "    return sm.sin(x) / x\n",
    "\n",
    "\n",
    "f(x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# And its graph:\n",
    "x_numerical = np.linspace(-5, 5)\n",
    "px.line(x=x_numerical, y=np.vectorize(f, otypes=[float])(x_numerical))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# It has a removable singularity at 0, of the form 0/0, which gives NaN:\n",
    "f(x).subs(x, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# The derivative has the same issue:\n",
    "f(x).diff(x).subs(x, 0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "One thought to fix this might be to just push the denominator away from 0.  This does resolve the NaN, but it produces the wrong value! (Remember, the value at `x=0` should be 1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def f(x, eps):\n",
    "    return sm.sin(x) / (x + eps)\n",
    "\n",
    "\n",
    "f(x, eps).subs(x, 0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Similarly, the derivative is wrong, and actually diverges (it should be 0):"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f(x, eps).diff(x).subs(x, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f(x, eps).diff(x).subs(x, 0).limit(eps, 0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Instead, what we want to do is _perturb the input away from the singularity_.  Effectively, we're shifting the graph of the function to the left.  For removable singularities in well-behaved functions, the error introduced by this is proportional to epsilon.  That looks like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def f(x, eps):\n",
    "    x_safe = x + eps\n",
    "    return sm.sin(x_safe) / x_safe\n",
    "\n",
    "\n",
    "f(x, eps).subs(x, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f(x, eps).subs(x, 0).limit(eps, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f(x, eps).diff(x).subs(x, 0)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "f(x, eps).diff(x).subs(x, 0).limit(eps, 0)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Automating Verification\n",
    "\n",
    "We can also write a function that automatically checks that you've used epsilon correctly, using SymPy's ability to take derivatives and limits as we've been doing above.  That looks something like this - similar functions are available in SymForce in [symforce.test_util.epsilon_handling](../api/symforce.test_util.epsilon_handling.html)."
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def is_epsilon_correct(func, singularity=0, limit_direction=\"+\"):\n",
    "    \"\"\"\n",
    "    Check epsilon handling for a function that accepts a single value and an\n",
    "    epsilon.\n",
    "\n",
    "    For epsilon to be handled correctly, the function must:\n",
    "        1) evaluate to a non-singularity at x=singularity given epsilon\n",
    "        2) linear approximation of the original must match that taken with\n",
    "           epsilon then substituted to zero\n",
    "    \"\"\"\n",
    "    # Create symbols\n",
    "    x = sm.Symbol(\"x\", real=True)\n",
    "    epsilon = sm.Symbol(\"epsilon\", positive=True)\n",
    "\n",
    "    is_correct = True\n",
    "\n",
    "    # Evaluate expression\n",
    "    expr_eps = func(x, epsilon)\n",
    "    expr_raw = expr_eps.subs(epsilon, 0)\n",
    "\n",
    "    display(\"Expressions (raw / eps):\")\n",
    "    display(expr_raw)\n",
    "    display(expr_eps)\n",
    "\n",
    "    # Sub in zero\n",
    "    expr_eps_at_x_zero = expr_eps.subs(x, singularity)\n",
    "    if expr_eps_at_x_zero == sm.S.NaN:\n",
    "        display(\"[ERROR] Epsilon handling failed, expression at 0 is NaN.\")\n",
    "        is_correct = False\n",
    "\n",
    "    # Take constant approximation at singularity and check equivalence\n",
    "    value_x0_raw = sm.simplify(expr_raw.limit(x, singularity, limit_direction))\n",
    "    value_x0_eps = expr_eps.subs(x, singularity)\n",
    "    value_x0_eps_sub2 = sm.simplify(value_x0_eps.limit(epsilon, 0))\n",
    "    if value_x0_eps_sub2 != value_x0_raw:\n",
    "        display(\n",
    "            f\"[ERROR] Values at x={singularity} do not match (raw / eps / eps.limit):\",\n",
    "        )\n",
    "        display(value_x0_raw)\n",
    "        display(value_x0_eps)\n",
    "        display(value_x0_eps_sub2)\n",
    "        is_correct = False\n",
    "\n",
    "    # Take linear approximation at singularity and check equivalence\n",
    "    derivative_x0_raw = sm.simplify(\n",
    "        expr_raw.diff(x).limit(x, singularity, limit_direction),\n",
    "    )\n",
    "    derivative_x0_eps = expr_eps.diff(x).subs(x, singularity)\n",
    "    derivative_x0_eps_sub2 = sm.simplify(derivative_x0_eps.limit(epsilon, 0))\n",
    "    if derivative_x0_eps_sub2 != derivative_x0_raw:\n",
    "        display(\n",
    "            f\"[ERROR] Derivatives at x={singularity} do not match (raw / eps / eps.limit):\",\n",
    "        )\n",
    "        display(derivative_x0_raw)\n",
    "        display(derivative_x0_eps)\n",
    "        display(derivative_x0_eps_sub2)\n",
    "        is_correct = False\n",
    "\n",
    "    return is_correct"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test sin(x) / x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Original function fails, singular at x=0\n",
    "assert is_epsilon_correct(lambda x, eps: sm.sin(x) / x) == False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Original broken attempt\n",
    "assert is_epsilon_correct(lambda x, eps: sm.sin(x) / (eps + x)) == False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Additive on top/bottom works\n",
    "assert is_epsilon_correct(lambda x, eps: (eps + sm.sin(x)) / (eps + x)) == True"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Replacing all x with (x + eps) works\n",
    "assert is_epsilon_correct(lambda x, eps: sm.sin(x + eps) / (x + eps)) == True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test (1 - cos(x)) / x"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Original fails, singular at 0\n",
    "assert is_epsilon_correct(lambda x, eps: (1 - sm.cos(x)) / x) == False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Value passes if we just replace the denominator, because this one is\n",
    "# ~ x**2 / x unlike the above which is ~ x / x\n",
    "assert is_epsilon_correct(lambda x, eps: (1 - sm.cos(x)) / (x + eps)) == False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Derivative also passes if we replace both\n",
    "assert is_epsilon_correct(lambda x, eps: (1 - sm.cos(x + eps)) / (x + eps)) == True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test x / sqrt(x**2)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Original fails, singular at 0\n",
    "assert is_epsilon_correct(lambda x, eps: x / sm.sqrt(x ** 2)) == False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Broken fix #1\n",
    "assert is_epsilon_correct(lambda x, eps: x / sm.sqrt(x ** 2 + eps ** 2)) == False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Broken fix #2\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: (x + eps) / sm.sqrt(x ** 2 + eps ** 2),\n",
    "    )\n",
    "    == False\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Broken fix #3, ugh\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: (x + eps) / (eps + sm.sqrt(x ** 2 + eps ** 2)),\n",
    "    )\n",
    "    == False\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Working if you again replace all x with x + eps\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: (x + eps) / sm.sqrt((x + eps) ** 2),\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test acos(x) / sqrt(1 - x^2) at 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Original fails, singular at 1\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: sm.acos(x) / sm.sqrt(1 - x ** 2),\n",
    "        singularity=1,\n",
    "    )\n",
    "    == False\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Working if you replace all x with x + eps\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: sm.acos(x + eps) / sm.sqrt(1 - (x + eps) ** 2),\n",
    "        singularity=1,\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Test atan2(0, x)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Original is singular\n",
    "assert is_epsilon_correct(lambda x, eps: sm.atan2(0, x)) == False"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# This works\n",
    "assert is_epsilon_correct(lambda x, eps: sm.atan2(0, x + eps)) == True"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Handling negative x"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "If we consider an example from above like $sin(x + \\epsilon) / (x + \\epsilon)$, this can easily be singular for a negative $x$, specifically where $x = -\\epsilon$. If $x$ were always negative, we could do $sin(x - \\epsilon) / (x - \\epsilon)$.\n",
    "\n",
    "So to handle both cases we can use the sign function as: $sin(x + sign(x) * \\epsilon) / (x + sign(x) * \\epsilon)$. This gives us the behavior that it always pushes $x$ away from zero because $sign(+) = 1$ and $sign(-) = -1$. However, $sign(0) = 0$ (at least, by SymPy's definition) which breaks the original zero point. To resolve this we can make a \"no zero\" version that arbitrarily picks the positive direction when exactly at zero.  We'll call this `sign_no_zero(x)`, or $snz(x)$\n",
    "\n",
    "We can implement this cleverly with the help of a min function:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def sign_no_zero(x):\n",
    "    # type: (sf.Scalar) -> sf.Scalar\n",
    "    \"\"\"\n",
    "    Returns -1 if x is negative, 1 if x is positive, and 1 if x is zero (given\n",
    "    a positive epsilon).\n",
    "    \"\"\"\n",
    "    return 2 * sm.Min(sm.sign(x), 0) + 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Test for sin(x) / x\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: (sm.sin(x + eps * sign_no_zero(x))) / (x + eps * sign_no_zero(x))\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Test for x / sqrt(x**2)\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: (x + eps) / sm.sqrt((x + eps) ** 2),\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Test for atan2(0, x)\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: sm.atan2(0, x + eps * sign_no_zero(x)),\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Generalization"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "So far it seems like for a function $f(x)$ that is singular at $x=0$, the expression $f(x + \\epsilon * snz(x))$ for a small positive $\\epsilon$ will be non-singular and retain the same linear approximiation as $f(x)$. So we can easily write a function that does this substitution:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def add_epsilon_sign(expr, var, eps):\n",
    "    return expr.subs(var, var + eps * sign_no_zero(var))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Check known example\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: add_epsilon_sign(sm.sin(x) / x, x, eps),\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Try some more complicated thing nobody wants to epsilon by hand\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: add_epsilon_sign(\n",
    "            (x + sm.sin(x) ** 2) / (x * (1 - 1 / x)),\n",
    "            x,\n",
    "            eps,\n",
    "        )\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "Another common case is a function $f(x)$ that is singular at $|x| = 1$, where we expect $x$ to be between $-1$ and $1$.  In this case we can use a similar idea, using $f(x - \\epsilon * snz(x))$.  A function to do this would be:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def add_epsilon_near_1_sign(expr, var, eps):\n",
    "    return expr.subs(var, var - eps * sign_no_zero(var))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Check known example\n",
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: add_epsilon_near_1_sign(\n",
    "            sm.cos(x) / sm.sqrt(1 - x ** 2),\n",
    "            x,\n",
    "            eps,\n",
    "        ),\n",
    "        singularity=1,\n",
    "        limit_direction=\"-\",\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Clamping\n",
    "\n",
    "We could also imagine clamping away from the singularity, instead of shifting with addition.  That would look like this:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "def add_epsilon_max(expr, var, eps):\n",
    "    return expr.subs(var, sign_no_zero(var) * sm.Max(eps, sm.Abs(var)))\n",
    "\n",
    "\n",
    "def add_epsilon_near_1_clamp(expr, var, eps):\n",
    "    return expr.subs(var, sm.Max(-1 + eps, sm.Min(1 - eps, var)))"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "However, this will always give a derivative of 0 at the singularity - since the derivative of `sign_no_zero(var) * sm.Max(eps, sm.Abs(var))` with respect to `var` is 0 there.  We can see this with an example function, whose derivative at the singularity should be 1.  `add_epsilon_sign` handles this correctly:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: add_epsilon_sign((sm.sin(x) + x ** 2) / x, x, eps),\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "`add_epsilon_max` gives the correct value, but not the correct derivative:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda x, eps: add_epsilon_max((sm.sin(x) + x ** 2) / x, x, eps),\n",
    "    )\n",
    "    == False\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Caveats and Limitations\n",
    "\n",
    "So far we've assumed the only singularity is at $x=0$, but everything above works just fine for singularities at other locations, for instance with the function $1/(1-x)$ for $x=1$.  What isn't handled well is functions that have multiple singularities, like $1/((x-1)(x-2)(x-3))$.  You can _compose_ functions using the singularity handling method above just fine, and everything will be safe.  This is almost always all that you need, but if you do encounter a function with multiple singularities that can't be rewritten as a composition of functions with fewer singularities, that will require something more complex.\n",
    "\n",
    "It's also easy to construct singularities where this method works symbolically, but relies on values like $\\epsilon^2$ which are too small in actual floating point implementations (see the `Pose2_SE2.to_tangent` section below).  And we could have a composite of multiple functions, so it's hard to have global visibility into what all your singular points are. Sometimes parameters have a fixed range, sometimes things are normalized, etc.\n",
    "\n",
    "So a remaining open question is: how well can we generalize this single variable example with a single known singularity to multiple variables and only locally known singularities like at the places we add a division, square root, or atan2 operation?"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Case Studies"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Pose2_SE2.from_tangent\n",
    "\n",
    "`Pose2_SE2.from_tangent` before epsilon handling looks like:\n",
    "\n",
    "```python\n",
    "def pose2_from_tangent(v):\n",
    "    theta = v[0]\n",
    "    R = Rot2.from_tangent([theta])\n",
    "    \n",
    "    a = sm.sin(theta) / theta\n",
    "    b = (1 - sm.cos(theta)) / theta\n",
    "    \n",
    "    t = geo.Vector2(a * v[1] - b * v[2], b * v[1] + a * v[2])\n",
    "    return geo.Pose2(R, t)\n",
    "```\n",
    "\n",
    "This has singularities in both `a` and `b` that we'd like to fix.  The initial version used:\n",
    "\n",
    "```python\n",
    "a = sm.sin(theta) / (theta + epsilon)\n",
    "b = (1 - sm.cos(theta)) / (theta + epsilon)\n",
    "```\n",
    "\n",
    "For `a`, this falls under the $sin(x) / x$ example above, and `b` falls under the $(1 - cos(x)) / x$ example above, so we can use the fixes from those examples, modified to handle negative $x$ as in the Generalization section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Pose2_SE2.to_tangent\n",
    "\n",
    "`Pose2_SE2.to_tangent` before epsilon handling looks like:\n",
    "\n",
    "```python\n",
    "def pose2_to_tangent():\n",
    "    theta = self.R.to_tangent()[0]\n",
    "    halftheta = 0.5 * theta\n",
    "    a = (halftheta * sm.sin(theta)) / (1 - sm.cos(theta))\n",
    "    \n",
    "    V_inv = Matrix[[a, halftheta], [-halftheta, a]])\n",
    "    t_tangent = V_inv * self.t\n",
    "    return [theta, t_tangent[0], t_tangent[1]]\n",
    "```\n",
    "\n",
    "This has a singularity in `a` that looks like:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda theta, eps: (0.5 * theta * sm.sin(theta)) / (1 - sm.cos(theta)),\n",
    "    )\n",
    "    == False\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "We might think this can be fixed with with the $x = x + \\epsilon$ trick:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda theta, eps: add_epsilon_sign(\n",
    "            (0.5 * theta * sm.sin(theta)) / (1 - sm.cos(theta)), theta, eps\n",
    "        )\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "But in practice, the denominator's taylor series is $(1 - (1 - 0.5 \\epsilon^2))$, and if $\\epsilon$ is near machine precision, then the denominator will end up as exactly `1 - 1 == 0`.  This might be solved by adding $sqrt(\\epsilon)$ instead, but that would introduce a large amount of error.  Instead, we do some algebra:\n",
    "\n",
    "$$\n",
    "\\begin{align*}\n",
    "\\frac{0.5 \\theta \\sin(\\theta)}{1 - \\cos(\\theta)}\n",
    "&= \\frac{0.5 \\theta \\sin(\\theta)}{1 - \\cos(\\theta)} \\frac{1 + \\cos(\\theta)}{1 + \\cos(\\theta)} \\\\\n",
    "&= \\frac{0.5 \\theta \\sin(\\theta) (1 + \\cos(\\theta))}{1 - \\cos^2(\\theta)} \\\\\n",
    "&= \\frac{0.5 \\theta \\sin(\\theta) (1 + \\cos(\\theta))}{\\sin^2(\\theta)} \\\\\n",
    "&= \\frac{0.5 \\theta (1 + \\cos(\\theta))}{\\sin(\\theta)}\n",
    "\\end{align*}\n",
    "$$\n",
    "\n",
    "Then, the only singularity is at $\\sin(\\theta) = 0$, which we can fix with:"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "assert (\n",
    "    is_epsilon_correct(\n",
    "        lambda theta, eps: (0.5 * (theta + eps) * (1 + sm.cos(theta))) / (sm.sin(theta) + eps)\n",
    "    )\n",
    "    == True\n",
    ")"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### Rot3.to_tangent\n",
    "\n",
    "`Rot3.to_tangent` before epsilon handling looks like:\n",
    "\n",
    "```python\n",
    "def logmap():\n",
    "    norm = sm.sqrt(1 - self.q.w**2)\n",
    "    tangent = 2 * self.q.xyz / norm * sm.acos(self.q.w)\n",
    "    return tangent\n",
    "```\n",
    "\n",
    "Ignoring the `q.xyz` variables, the function has a singularity at `w == 1`, of the form\n",
    "$$\n",
    "\\frac{\\arccos(w)}{\\sqrt{1 - w^2}}\n",
    "$$\n",
    "\n",
    "which can be fixed using either the `add_epsilon_near_1_sign` or `add_epsilon_near_1_clamp` methods described in the Generalization section."
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "## Multivariate functions"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "x, y = sm.symbols(\"x, y\", real=True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Looking at a normalization function\n",
    "epsilon = sm.Symbol(\"epsilon\")\n",
    "expr = x / sm.sqrt(x ** 2 + y ** 2)\n",
    "sm.series(expr, x, n=2)\n",
    "sm.simplify(expr.diff(x).limit(x, 0))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Simulate a normalization where y = 0\n",
    "is_epsilon_correct(lambda x, eps: sm.Matrix([x + eps, 0]).normalized()[0])"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "is_epsilon_correct(\n",
    "    lambda x, eps: (x + eps) / sm.sqrt((x + eps) ** 2 + (0 + eps) ** 2),\n",
    ")"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "is_epsilon_correct(lambda x, eps: (x + eps) / sm.sqrt(eps ** 2 + (x + eps) ** 2))"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "is_epsilon_correct(lambda x, eps: (x + eps) / sm.sqrt(eps ** 2 + (x + eps) ** 2))"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3.8.9 64-bit",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "vscode": {
   "interpreter": {
    "hash": "36f0e22b03afe0406e010af8b04d290f2434e9abbb5b497280550ee7446112c1"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
